#include "cityStructs.h"
#include "time.h"

using namespace std;

Graph *makeGraph(int vertxs, int N, int n);
void edgeAdd(Graph *graph, int N, int n);
Vertex *findVertxs(Graph *graph, int ID);
carqueueList *carqueuePopulate(int n, int maxvertex, int max_specCars);
void graphPopulate(Graph *graph, carqueueList *_streetCars);
dijkstraList *Dijkstra(Graph *graph, int startID, int N);
void pathUpdate(Graph *graph, carqueueList *_streetCars, int N);
void bubbleSort(streetList *_streetList, int n);
void semStart(Graph *graph, carqueueList *_streetCars, int n);

/* Crea el grafo de la ciudad a partir de la cantidad de vertices, N y cantidad de autos en el cityConfig */
Graph *makeGraph(int vertxs, int N, int n){
	Graph *graph=new Graph();
	for(int i=vertxs;i>0;i--) //Agrego los vertices al grafo
		graph->add_vertex(i);
	edgeAdd(graph, N, n);
	return graph;
};

/* Agrega las aristas a los vertices del grafo */
void edgeAdd(Graph *graph, int N, int n){
	printf("Construccion del mapa de %i vertices\n\n", N*N);
	int ID_aux;
	int m=1;
	Vertex *o = graph->get_graphStart(); //Vertice Origen
	Vertex *d; //Vertice Destino
	int maxvertex = N*N; //Cantidad de vertices disponibles
	
	while (m<=N){ //m reprensenta los niveles del grafo
			
		for (int i=1; i<=N; i++){ //Recorro la matriz N veces por nivel
			ID_aux = o->get_vertextID(); //Se guarda el ID del vertice de origen
			
			if ((m%2 != 0) && (m*N!= ID_aux)){ //Si se cumple la condicion el destino de la arista es el vertice siguiente, cuando llegamos al ultimo vertice no va a cumplir la condicion
				d=o->get_next_vertex(); //Guarda en el destino el vertice inmediato mas proximo
					
				if(ID_aux < N){  //Desde los vertices 1 al N habran avenidas
					graph->add_ave(o,d,n,3);
					printf("Avenida de %i - %i y %i - %i %\n", o->get_vertextID(), d->get_vertextID(), d->get_vertextID(), o->get_vertextID());
				}
					
				else{ //Agrego calles para los vertices m*N con 1<m<N con m par, y se asignan de manera decreciente
					graph->add_edge(o,d,n,1);
					printf("Calle de %i - %i\n", o->get_vertextID(), d->get_vertextID());
				}
			}
			
			if ((m%2 == 0) && ((m-1)*N+1 != ID_aux)){
				d = findVertxs(graph, ID_aux-1); //Encuentra los vertices cuyo ID apunta al anterior inmediato, menos el primero
					
				if(ID_aux > maxvertex-(N)){ //Tomando el valor de maxvertex, agrego avenidas de forma decreciente hasta maxvertex-N
					graph->add_ave(o,d,n,3);
					printf("Avenida de %i - %i y %i - %i %\n", o->get_vertextID(), d->get_vertextID(), d->get_vertextID(), o->get_vertextID());
				}
					
				else{ //Agrego calles para los vertices m*N con 1<m<N con m impar, y se asignan de manera creciente
					graph->add_edge(o, d, n, 1);
					printf("Calle de %i - %i\n", o->get_vertextID(), d->get_vertextID());
				}
			}
			
			if ((ID_aux%2 == 0) && (ID_aux < (maxvertex-N))){	
				d=o; //Asigna el destino igual que el origen
					
				for(int j=0;j<N;j++)	//Con el for apunto del vertice de origen N veces mas adelante
					d=d->get_next_vertex(); //El destino se asigna con el vertice inmediato siguiente
					
				if(m*(N) == ID_aux){ //Cuando llego al ultimo vertice de nivel m, agrego una avenida que se conecta con el vertice N posiciones mas adelante solo cuando m<N
					graph->add_ave(o,d,n,2);
					printf("Avenida de %i - %i y %i - %i %\n", o->get_vertextID(), d->get_vertextID(), d->get_vertextID(), o->get_vertextID());
				}
					
				else{ //Agrego una calle que conecta al vertice N posiciones mas adelante, si el vertice es par, si m<N
					graph->add_edge(o,d, n, 1);
					printf("Calle de %i - %i\n", o->get_vertextID(), d->get_vertextID());
				}
			}
			
			if ((ID_aux%2 != 0) && (ID_aux > N)){
				d = findVertxs(graph, ID_aux-(N)); //Asigna al destino los vertices que sean N veces atras del mismo
					
				if ((m-1)*N+1 == ID_aux){ //Conecto el primer vertice del nivel m con el vertice N veces anterior del mismo, solo cuando m>1
					graph->add_ave(o,d, n, 2);
					printf("Avenida de %i - %i y %i - %i %\n", o->get_vertextID(), d->get_vertextID(), d->get_vertextID(), o->get_vertextID());
				}
				else{ //Agrego una calle que conecta al vertice N posiciones mas atras, si el vertice es impar y m>2
					graph->add_edge(o,d, n, 1);
					printf("Calle de %i - %i\n", o->get_vertextID(), d->get_vertextID());
				}		
			}
			o = o->get_next_vertex(); //Para la proxima iteracion toma el siguiente vertice como currentPos inicial
		}
		m++; //Para pasar de niveles en el grafo
	}
	system("Pause");
}


/* Busca vertices del grafo a partir de la ID de dicho vertice */
Vertex *findVertxs(Graph *graph, int ID){
	Vertex *v = graph->get_graphStart();
	if(!graph->is_empty()){
		if (v->get_vertextID() == ID)
			return v;
		else
			v = findVertxs(graph->remain(),ID);
	}
	else
		return 0;
	return v;
}

/* Crea una lista de vehiculos */
carqueueList *carqueuePopulate(int n, int maxvertex, int max_specCars){
	system("Pause");
	printf("Tabla de autos:\n\n"); //Hay que cambiar esto
	carqueueList *l = new carqueueList(); //Creo la lista de autos
	streetCars *m; //Nodos
	int type; //type=0 son autos comunes, type=1 son autos spec
	int startpos; //Para cargar el origen
	int endpos; //Para cargar el destino
	int specCars = 0; //Cantidad de autos especiales
	srand(time(NULL));
	
	for(int i=n;i>0;i--){ //Creo la lista de autos con sus posiciones
		startpos = 1+rand()%maxvertex; // De 1 a maxvertex
		endpos = 1+rand()%maxvertex;
		type = 0;
		
		if(specCars < max_specCars){ //Cuando agrego el numero de autos especiales, paso a los autos comunes
			type = 1; 
			specCars++;
		}
		
		m = new streetCars(i, type, startpos, endpos); //Construyo el nodo con los paramemetros
		l->add(m); //Agrego el nodo a la lista
		printf("\nID de Auto: %i\nPosicion Inicial: %i\nPosicion de Destino: %i\nTipo de auto (0 Normal, 1 Especial): %i\n", m->get_ID(), m->get_startpos(), m->get_endpos(), m->get_tipo());
	}
		
	return l;
};

/* Carga las aristas del grafo con los autos */
void graphPopulate(Graph *graph, carqueueList *_streetCars){

	if (!_streetCars->checkempty()){ //Solo si la lista no es vacia
		streetCars *m = _streetCars->head(); //Guarda el tope en m
		Vertex *v = findVertxs(graph, m->get_startpos()); //Guarda la posicion inicial del auto tope de m en v
		Edge *a = v->get_edge(); //Guarda la arista correspondiente al vertice anterior en a
		carqueueQueue *c; //Cola de autos que contiene a _streetCars
		c = a->get_carqueueQueue(); //Devuelve en c la cola de autos que estaban en la arista
		c->_queue(m); //Encola el tope en la cola de autos de la arista del vertice
		m->set_pos(a->get_endpos()->get_vertextID()); //Posiciona al auto tope en el vertice donde termina la arista	
		graphPopulate(graph, _streetCars->remain()); //Recursivamente cargo el resto de la lista
	}
}

/* Dijkstra: reordena el recorrido de los autos correspondientemente a la distancia minima entre los vertices y la carga de los arcos */
dijkstraList *Dijkstra(Graph *graph, int startID, int N){
	int maxvertex = (N)*(N); //Cantidad maxima de vertices
	int endpos_ID; //ID del vertice final
	int auxID; //ID auxiliar para intercambiar las colas de caminos de cada nodo
	dijkstraList *vertxs = new dijkstraList(); //Lista de Dijkstra que esta compuesta por vertices
	Vertex *endVertx; //Vertice final
	dijkstraNode *endNode; //Nodo de Dijsktra correspondiente al vertice final
	intQueue *c; //Cola de enteros donde se almacena el camino
	intQueue *c_aux = new intQueue(); //Guarda valores de la currentPos inicial desenconlada
	intQueue *start_carqueueQueue; //La cola inicial
	intQueue *end_carqueueQueue; //La cola con el camino nuevo (Resultado de la aplicacion del algoritmo de Dijkstra)
	
	//Se inicializa una lista de Dijkstra cuyos arcos tienen un peso inicial eq a INF, y a los vertices se los pone como no visitados
	//El objetivo de esto es forzar a que se toma el peso inicial arco una primera vez en la iteracion de semaforo
	for (int i=maxvertex; i>0; i--){ 
		vertxs->add(i);
		vertxs->dijkstraList_getstart()->set_seen(0);
		vertxs->dijkstraList_getstart()->set_span(INF); 
		c = new intQueue(); //Cola que almacena el camino minimo de cada vertice
		vertxs->dijkstraList_getstart()->set_intQueue(c);
		if (i==startID)
			vertxs->dijkstraList_getstart()->set_span(0); //Por definicion de Dijsktra el nodo inicial siempre tiene peso 0
	}

	dijkstraNode *startpos;
	Vertex *currentVertx;
	Edge *edge; //Tomo la arista del nodo inicial
	
	//Recorro todas las aristas del grafo utilizando el algoritmos de Dijkstra, el cual modifica las distancias por el valor minimo a recorrer
	for (int i=0; i<maxvertex; i++){
		startpos = vertxs->find_vmin(); //Cambio el nodo activo por el nodo proximo con la distancia mas corta dependediendo del peso de los arcos
		currentVertx = findVertxs(graph, startpos->get_data());	//Cambio el vertice actual por el vertice a obtenido anteriormente
		edge = currentVertx->get_edge(); //Leo la arista correspondiente al vertice
		
		while(!edge->is_empty()){ //Mientras la arista tenga autos
			endVertx = edge->get_endpos();	//Obtengo el vertice final de la arista
			endpos_ID = endVertx->get_vertextID();
			endNode = vertxs->find_dijkstraNode(endpos_ID); //Obtengo el vertice final de Disjktra
			if(endNode->get_span() > edge->get_weight()+startpos->get_span()){ //Si la distancia del nodo de Dijkstra es mayor que la suma del peso de la arista mas el de la currentPos inicial
				endNode->set_span(edge->get_weight()+startpos->get_span()); //Modifico el valor de la distancia que hay en los vertice de Dijkstra
				//Modifico la cola de caminos ya que encontre un camino con distancia mas corta
				start_carqueueQueue = startpos->get_intQueue();
				end_carqueueQueue = endNode->get_intQueue();
				//Desencolo los elementos de la cola final
				while(!end_carqueueQueue->emptyqueue())							
					end_carqueueQueue->_unqueue();
				
				//Encolo los valores en la cola inicial en la final, y despues vacio la cola inicial
				while(!start_carqueueQueue->emptyqueue()){
					auxID =start_carqueueQueue->top();
					c_aux->_queue(auxID);
					end_carqueueQueue->_queue(auxID);
					start_carqueueQueue->_unqueue();
				}
				
				//Agrego el nodo de Dijsktra predecesor al vertice actual en la cola final
				end_carqueueQueue->_queue(startpos->get_data());
				
				//Restauramos la cola inicial desde la cola auxiliar
				while(!c_aux->emptyqueue()){
					auxID = c_aux->top();
					start_carqueueQueue->_queue(auxID);
					c_aux->_unqueue();
				}
			}
			edge = edge->get_next_edge();
		}	
		startpos->set_seen(1);		
	}
	return vertxs;
}

void pathUpdate(Graph *graph, carqueueList *_streetCars, int N){ //Funcion Recursiva
	dijkstraList *minSpan; //Conjunto de vertices que devuelve el algoritmo de Dijkstra, contiene ademas la distancia a otros vertices
	dijkstraNode *endpos; //Un nodo de Dijkstra que encola el camino que tiene que realizar los vehiculos
	intQueue *currentQueue; //El camino actual del auto
	intQueue *newQueue; //El camino que debe asignarle al auto
	
	if(!_streetCars->checkempty()){
		int currentPos = _streetCars->head()->get_pos(); //Devuelve a que vertice va la arista actual donde se encuentra el auto
		minSpan = Dijkstra(graph, currentPos, N); //Utilizando el algoritmo de Dijkstra calcula los recorridos minimos desde la currentPos actual hasta el destino
		endpos = minSpan->find_dijkstraNode(_streetCars->head()->get_endpos());
		newQueue = endpos->get_intQueue();
		currentQueue = _streetCars->head()->get_recorrido();
		
		while(!currentQueue->emptyqueue()) 
			currentQueue->_unqueue(); //Desencolo la cola actual del auto
		
		printf("El minimo camino para el auto con ID %i es: ",_streetCars->head()->get_ID());
				
		while(!newQueue->emptyqueue()){ //Encolo los nuevos nodos obtenidos por Dijkstra
			currentQueue->_queue(newQueue->top());
			cout<<newQueue->top()<<" - ";
			newQueue->_unqueue();
		}
				
		currentQueue->_queue(_streetCars->head()->get_endpos()); //Se encola el ultimo nodo
		cout<<_streetCars->head()->get_endpos()<<endl;
			
		pathUpdate(graph, _streetCars->remain(), N); //Recursivamente seguimos con el proximo auto
	}	
}

/* Para el reordenamiento de los autos dentro de los semaforos */
void bubbleSort(streetList *_streetList, int n){ //Lento pero seguro
	int loadEdge;
	if (n > 0){
		streetNode *dNode = _streetList->streetList_getstart();
				
		while(dNode->get_next()->get_next()!=NULL){
			loadEdge = dNode->get_edge()->get_carqueueQueue()->get_maxcars();
			if(loadEdge > dNode->get_next()->get_edge()->get_carqueueQueue()->get_maxcars())
				_streetList->_swapdata(dNode, dNode->get_next()); //Intercambiamos los punteros de las aristas de los nodos 
			dNode = dNode->get_next();
		}
		bubbleSort(_streetList, --n);
	}
}

Stack *semSort(Graph *graph){
	system("Pause");
	printf("Tabla de Ranking de semaforos\n");
	int checkEdges = 0; //Inicio desde arista 0
	Stack *stack = new Stack(); 
	streetList *_edges = new streetList();
	Edge *edge; //Arista
	Vertex *_vertx = graph->get_graphStart(); //Agarro la arista principal del grafo
	
	while(!_vertx->is_empty()){ //Arista correspondiente al vertice
		edge = _vertx->get_edge();
		
		while(!edge->is_empty()){ //Recorro todas las aristas
			_edges->add(edge);  //Cargo todos los punteros de las aristas del grafo en una lista de aristas, debera ser ordenada de acuerdo a la carga de autos
			checkEdges++; //Conteo de aristas
			edge = edge->get_next_edge(); //Avanza a la proxima arista 
		}
		_vertx = _vertx->get_next_vertex(); //Se mueve entre los vertices
	}
	bubbleSort(_edges, checkEdges); //Paso al metodo de ordenamiento la lista de aristas y su cantidad final
	streetNode *dNode = _edges->streetList_getstart();
	
	while (!dNode->checkempty()){
		stack->_stack(dNode->get_edge()); //Tomo la arista del nodo primero en la cola
		dNode = dNode->get_next(); //Recibo la arista siguiente 
		printf("Vertice inicial de arista: %i Vertice Final de la arista: %i Cant de autos: %i\n", stack->top()->get_startpos()->get_vertextID(), stack->top()->get_endpos()->get_vertextID(), stack->top()->get_carqueueQueue()->get_maxcars());
	}
	return stack; //Devuelvo la pila una vez que se completa el ciclo
}

/* Habilita semaforos, mueve los autos */
void semStart(Graph *graph, carqueueList *_streetCars, int n){	
	system("Pause");
	printf("Movimiento de los autos por iteracion de semaforo:\n");
	Stack *stack = semSort(graph);
	int i;
	bool carMovmtDone; 
	Edge *startEdge; //Arista inicial
	Edge *endEdge; //Arista final
	Vertex *endVertx; //Vertice destino
	carqueueQueue *start_carqueueQueue; //Cola inicial
	carqueueQueue *end_carqueueQueue; //Cola final
	streetCars *car; //Cola de autos
	intQueue *_path; //Contiene el recorrido optimo hasta la currentPos final
	int nextVertx;
	int edgeType;
	
	while(!stack->_checkempty())
	{
		startEdge = stack->top(); //Mientras la cola este no vacia tomo la primera arista
		edgeType = startEdge->get_edgetype(); //Cantidad de vias disponibles
		endVertx = findVertxs(graph, startEdge->get_endpos()->get_vertextID());
		start_carqueueQueue = startEdge->get_carqueueQueue(); //La cola que contiene los autos a ser movidos
		i = 0;

		while((!start_carqueueQueue->emptyqueue()) && (i<n*edgeType)){ //While mientras la cola este llena y pasen la cantidad maxima permitida de autos
			car = start_carqueueQueue->get_start(); //Tomo el primer auto de la cola de autos
			_path = car->get_recorrido(); //Tomo el recorrido de dicho auto
			_path->_unqueue(); //Desencolo el primer nodo ya que por Dijsktra no es necesario (peso 0) 
			
			if(!_path->emptyqueue()){ //Si la cola esta vacia entonces llego a su currentPos final
				nextVertx = _path->top(); //Tomo el proximo vertice para saber que aristas tiene asociadas y luego decide a cual pasar
				carMovmtDone = false; //Variable de control
				endEdge = endVertx->get_edge(); //Primera arista del vertice
				
				while((!endEdge->is_empty()) && (!carMovmtDone)){ //Ciclo que termina cuando el auto haya terminado su movimiento o hasta que se recorran todas las aristas
					end_carqueueQueue = endEdge->get_carqueueQueue(); //Posicion final donde llega el auto
					
					if((nextVertx == endEdge->get_endpos()->get_vertextID()) && (end_carqueueQueue->check_space()))	{ //Chequea que la arista siguiente se corresponda con el vertice siguiente y que haya espacio en la cola de la arista
						start_carqueueQueue->_unqueue(); //Desencola el auto de la cola inicial
						end_carqueueQueue->_queue(car); //Encola el auto en la cola final
						car->set_pos(endEdge->get_endpos()->get_vertextID()); //Vertice final correspondiente a la arista nueva final
						printf("ID de auto: %i desde %i hasta %i\n", car->get_ID(), startEdge->get_startpos()->get_vertextID(), startEdge->get_endpos()->get_vertextID());
						carMovmtDone = true; //El auto termino su moviemiento
					}
					endEdge = endEdge->get_next_edge(); //Se toma la proxima arista, que es la final
				}
			}
			
			else{
				printf("ID de auto: %i llego a su destino: %i\n", car->get_ID(), startEdge->get_endpos()->get_vertextID());
				start_carqueueQueue->_unqueue(); //Se retira el auto del grafo
				_streetCars->deletecar(car->get_ID()); //Se borra el auto de la lista de autos
			}
			i++;		
		}
		
		stack->_unstack(); //Desapilo la arista que ha sido habilitada por el semaforo
	}
}